// This sketch reads IMU data from the SparkFun 5DOF IMU found at:
// http://www.sparkfun.com/commerce/product_info.php?products_id=741
// The AREF pin is connected to 3.3V to better use the 10 bit A/D
// 90 degree roll left = +1G, roll right = -1G, level = 0G
// 90 degree pitch down = +1G, pitch up = -1G, level = 0G
// Z axis, level = 1G, 90 degree = 0G and upside down = -1G

// How to calculate G
// G = ((ACC-(((MAX-MIN)/2)+MIN)+ZERO) * (1/(MAX-MIN)*2)

// Debug data 
#define BAUD 38400  

// Define pin assignments
#define LEDPIN 13
#define XACCELPIN 2 // I miswired my board, change order based on your setup
#define YACCELPIN 0
#define ZACCELPIN 1
#define XRATEPIN 4
#define YRATEPIN 3

// Accelerometer setup
// these A/D values depend on how well the sensors are mounted
// change these values to your unique configuration
#define XMIN 405
#define XMAX 607
#define YMIN 409
#define YMAX 618
#define ZMIN 403
#define ZMAX 611
float xAccMid = ((float)(XMAX-XMIN) / 2) + (float)XMIN;
float yAccMid = ((float)(YMAX-YMIN) / 2) + (float)YMIN;
float zAccMid = ((float)(ZMAX-ZMIN) / 2) + (float)ZMIN;
float xAccScaleFactor = 1 / (float)(XMAX - XMIN) * 2;
float yAccScaleFactor = 1 / (float)(YMAX - YMIN) * 2;
float zAccScaleFactor = 1 / (float)(ZMAX - ZMIN) * 2;
int xAccZero, yAccZero, zAccZero;
float xG, yG, zG;

// Gyro setup
// if AREF = 3.3V, then A/D is 931 at 3V and 465 = 1.5V
float GyroMid = 465;
// gyro range is from +/-500 deg/sec
float GyroScaleFactor = 1001 / (float)ZMAX;
int xGyroZero, yGyroZero;
float xRate, yRate;

unsigned long previousTime = 0;
unsigned long deltaTime = 0;
float voltageScaleFactor = 3.3 / 1024;

void setup() {
  digitalWrite(LEDPIN, HIGH);
  analogReference(EXTERNAL); // Current external ref is 3.3V
  pinMode(LEDPIN, OUTPUT);
  Serial.begin(BAUD);
  xAccZero = xAccMid - findZero(XACCELPIN);
  yAccZero = yAccMid - findZero(YACCELPIN);
  zAccZero = ZMAX - findZero(ZACCELPIN);
  xGyroZero = GyroMid - findZero(XRATEPIN);
  yGyroZero = GyroMid - findZero(YRATEPIN);
  pinMode(3,OUTPUT);
  digitalWrite(3,HIGH);
}

void loop() {
  previousTime = millis();

  // G = ((ACC-(((MAX-MIN)/2)+MIN)+ZERO) * (1/(MAX-MIN)*2)
  xG = (analogRead(XACCELPIN) - xAccMid + xAccZero) * xAccScaleFactor;
  yG = (analogRead(YACCELPIN) - yAccMid + yAccZero) * yAccScaleFactor;
  zG = (analogRead(ZACCELPIN) - zAccMid + zAccZero) * zAccScaleFactor;
  
  // measuredRate = (RATE-GYROMID+ZERO)*(1000/MAX)
  xRate = ((float)analogRead(XRATEPIN) - GyroMid + xGyroZero) * GyroScaleFactor;
  yRate = ((float)analogRead(YRATEPIN) - GyroMid + yGyroZero) * GyroScaleFactor;
  
  deltaTime = millis() - previousTime;

  digitalWrite(LEDPIN, HIGH);
  /*Serial.print(deltaTime);
  Serial.print(",");
  Serial.print(xAccel);
  Serial.print(",");
  Serial.print(yAccel);
  Serial.print(",");
  Serial.print(zAccel);
  Serial.print(",");*/
  // Serial.print doesn't work with float, so multiply by 1000 and convert to int for debug
  Serial.print((int)(xG*1000));
  Serial.print(",");
  Serial.print((int)(yG*1000));
  Serial.print(",");
  Serial.print((int)(zG*1000));
  Serial.print(",");
  Serial.print((long)(xRate*1000));
  Serial.print(",");
  Serial.println((long)(yRate*1000));
  digitalWrite(LEDPIN, LOW);  
}

float findZero(int channel) {
  float zero = 0;
  for (int i=0; i< 100; i++) zero += analogRead(channel);
  return (zero / 100);
}  
